#include "kuruma.h"
#include "user.h"
#include "command.h"

ClosedLoopMotor ml,mr; //左右电机


void kuruma_tick() { //此函数调用周期为5ms
	odometry_update(); //读取传感器数据，计算并更新位姿
	if(!handle_command())
		user_tick_run();
	clmotor_update(&ml); //刷新pid输出
	clmotor_update(&mr);
}

void kuruma_init(void) {
	odometry_init(TICK_TIME,&ml.speed,&mr.speed);
	
	pinMode(GPIO_PIN_PA4,OUTPUT);
	// 使能 TB6612
	digitalWrite(GPIO_PIN_PA4,1);

	clmotor_init(&ml, 
				GPIO_PIN_PD2, GPIO_PIN_PB3, 1040, 0, //encoder的两个引脚， 每转脉冲数， mt测速模式
				0.85,50,0.0,	TICK_TIME,		// kp,ki,kd
				GPIO_PIN_PB0,	// PWMA (TIM3_CH3)
				GPIO_PIN_PA0,   // AIN1
				GPIO_PIN_PA1);   // AIN2
	
	clmotor_init(&mr, 
				GPIO_PIN_PB4, GPIO_PIN_PB5, 1040, 0, //encoder的两个引脚， 每转脉冲数， mt测速模式
				0.85,50,0.0,	TICK_TIME,		// kp,ki,kd
				GPIO_PIN_PB1,	// PWMB (TIM3_CH4)
				GPIO_PIN_PC2,   // BIN2
				GPIO_PIN_PC3);   // AIN2
	
	// 配置定时器：1Mhz
    timerBegin(TIMER_2, 72, 1);
    timerAttachInterrupt(TIMER_2, kuruma_tick); 
    timerAlarmWrite(TIMER_2, 5000, 1);  
	printf("kuruma init done.\n");
	timerAlarmEnable(TIMER_2);
}

void kuruma_set_velocity(float linear, float angular) //(直行)线速度与(逆时针)角速度设定，解算为两轮速度，并设定目标值
{
    float vr = linear + angular * WHEEL_BASE_M / 2;
    float vl = linear - angular * WHEEL_BASE_M / 2;
	float rpm_r = vr * 60 / (2 * M_PI * WHEEL_RADIUS_M);
	float rpm_l = vl * 60 / (2 * M_PI * WHEEL_RADIUS_M);
    clmotor_set_target(&ml,rpm_l);
	clmotor_set_target(&mr,-rpm_r);
}

void kuruma_enable(void) {
	// 使能电机
    clmotor_enable(&ml);
    clmotor_enable(&mr);
}

void kuruma_disable(void) {
    clmotor_disable(&ml);
    clmotor_disable(&mr);
}
